#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "origincar_msg/msg/sign.hpp"
#include "std_msgs/msg/int32.hpp"
#include "ai_msgs/msg/perception_targets.hpp"
#include "cv_bridge/cv_bridge.h"
#include <zbar.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <sstream>

class QRNODE : public rclcpp::Node {
public:
    QRNODE() : Node("QR"), frame_count(0), x_offset(0), y_offset(0), samble(0), x(0), y(0) {
        subscription_fcos = this->create_subscription<ai_msgs::msg::PerceptionTargets>(
            "/hobot_dnn_detection", 10, std::bind(&QRNODE::callback, this, std::placeholders::_1));
        subscription = this->create_subscription<sensor_msgs::msg::CompressedImage>(
            "/image", 10, std::bind(&QRNODE::image_callback, this, std::placeholders::_1));
        subscription_sign = this->create_subscription<std_msgs::msg::Int32>(
            "/sign4return", 10, std::bind(&QRNODE::sign4return_callback, this, std::placeholders::_1));
        qrcode_pub = this->create_publisher<std_msgs::msg::Int32>
            ("/qrcode_data", 1);
        sign_switch_pub = this->create_publisher<origincar_msg::msg::Sign>("sign_switch", 10);
        cmd_pub = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
    }

private:
    int frame_count = 0;
    int x_offset;
    int y_offset;
    int samble;
    int x;
    int y;
    int task = 0;
    rclcpp::Subscription<ai_msgs::msg::PerceptionTargets>::SharedPtr subscription_fcos;
    rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr subscription;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign;
    rclcpp::Publisher<origincar_msg::msg::Sign>::SharedPtr sign_switch_pub;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr qrcode_pub;

    void sign4return_callback(const std_msgs::msg::Int32 msg){
        if (msg.data == -1){
            task = 1;
            RCLCPP_WARN(this->get_logger(), "qr_start");;
        }
    }

    void callback(const ai_msgs::msg::PerceptionTargets::SharedPtr msg) {
        if (task == 1){
        for (const auto& target : msg->targets) {
            if (!target.rois.empty()) {
                auto roi = target.rois[0].rect;
                x_offset = roi.x_offset;
                y_offset = roi.y_offset;
                int height = roi.height;
                int width = roi.width;
                std::string class_name = target.rois[0].type;

                if (class_name == "qr") {
                    x = x_offset + width;
                    y = y_offset + height;
                    samble = 1;
                }
            }
        }
        }
    }

    void image_callback(const sensor_msgs::msg::CompressedImage::SharedPtr image) {
        if (samble == 1) {
            auto qr_code_data = processimg(image);
            samble = 0;

            if (!qr_code_data.empty()) {
                auto cmd_msg = geometry_msgs::msg::Twist();
                cmd_msg.linear.x = 0.0;
                cmd_msg.angular.z = 0.0;
                cmd_pub->publish(cmd_msg);
                RCLCPP_INFO(this->get_logger(), "%s", qr_code_data[0].c_str());

                std::string qr_code_data2 = qr_code_data[0];
                if (qr_code_data2 == "ClockWise" || qr_code_data2 == "AntiClockWise") {
                    int sign_data = (qr_code_data2 == "ClockWise") ? 3 : 4;
                    origincar_msg::msg::Sign sign_msg;
                    sign_msg.sign_data = sign_data;
                    sign_switch_pub->publish(sign_msg);
                    task = 0;
                } else {
                    int qr_code_data3 = std::stoi(qr_code_data2);
                    std_msgs::msg::Int32 qrcode_msg;
                    qrcode_msg.data = qr_code_data3;
                    qrcode_pub->publish(qrcode_msg);
                    int sign_data = (qr_code_data3 % 2 == 1) ? 3 : 4;
                    origincar_msg::msg::Sign sign_msg;
                    sign_msg.sign_data = sign_data;
                    sign_switch_pub->publish(sign_msg);
                    task = 0;
                }
            } else {
                RCLCPP_WARN(this->get_logger(), "focs识别成功但解码失败");
            }
        }
    }

    std::vector<std::string> processimg(const sensor_msgs::msg::CompressedImage::SharedPtr& image) {
    cv::Mat cv_image = cv::imdecode(cv::Mat(image->data), cv::IMREAD_COLOR);
    if (cv_image.empty()) {
        RCLCPP_ERROR(this->get_logger(), "解码的图像为空");
        return {};
    }

    // 获取原始图像尺寸
    int img_width = cv_image.cols;
    int img_height = cv_image.rows;

    // 确保偏移量和坐标有效
    int safe_x_offset = std::max(0, x_offset);  // 确保x_offset不小于0
    int safe_y_offset = std::max(0, y_offset);  // 确保y_offset不小于0
    

    // 计算裁剪区域
    int roi_x, roi_y, roi_width, roi_height;
    roi_x = std::max(0,safe_x_offset-10);
    roi_y = std::max(0,safe_y_offset-10);
    roi_width = std::max(0,std::min(x+10,639) - roi_x) ;
    roi_height = std::max(0,std::min(y+10,479) - roi_y);
    cv::Rect roi(roi_x, roi_y, roi_width, roi_height);
    cv::Mat cropped = cv_image(roi);

    // 检查裁剪结果是否有效
    if (cropped.empty()) {
        std::cout << "illegal_image" << std::endl;
        return {};
    }

    std::vector<std::string> decoded_data;
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_QRCODE, zbar::ZBAR_CFG_ENABLE, 1);

    // Convert to grayscale and create zbar image
    cv::Mat gray;
    cv::cvtColor(cropped, gray, cv::COLOR_BGR2GRAY);
    int width = gray.cols;
    int height = gray.rows;
    zbar::Image zbar_image(width, height, "Y800", gray.data, width * height);
    scanner.scan(zbar_image);

    for (zbar::Image::SymbolIterator symbol = zbar_image.symbol_begin(); symbol != zbar_image.symbol_end(); ++symbol) {
        decoded_data.push_back(symbol->get_data());

        // Save the decoded image
        std::ostringstream filename;
        filename << "/root/frames/decoded_frame_" << frame_count++ << ".jpeg"; // Unique file name
        cv::imwrite(filename.str(), cropped); // Save image to disk
    }
    return decoded_data;
}
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<QRNODE>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
 